#include "FileSensor.h"
#include <iostream>
#include <process.h>

FileSensor::FileSensor (const QString &name,
		const QString &dataFile, const QString &intrinsicFile,
		const QString &extrinsicFile) :
		Sensor (name, intrinsicFile, extrinsicFile), _pTimer (NULL),
		_hasNewFrame (false), _inStream (dataFile.toStdString (), std::ios::binary),
		_isStarted (false) {
	if (!_inStream) {
		printf ("Cannot open the file %s", dataFile);
		throw (-1);
	}

	__sensorParameter.readFrom (_inStream);

	if (__sensorParameter.depthPixelWidth) {
		__depthMetaData = QImage (__sensorParameter.frameWidth,
				__sensorParameter.frameHeight, QImage::Format_RGB16);
		__depthMetaData.fill (Qt::black);
	}
	if (__sensorParameter.rgbPixelWidth) {
		__rgbMetaData = QImage (__sensorParameter.frameWidth,
				__sensorParameter.frameHeight, QImage::Format_ARGB32);
		__rgbMetaData.fill (Qt::black);
	}

	_frameDataSize = __sensorParameter.frameSize * (
			__sensorParameter.depthPixelWidth + __sensorParameter.rgbPixelWidth);
	_buf = new char[_frameDataSize];

	_pTimer = new QTimer (NULL);
	_pTimer->setSingleShot (true);
	connect (_pTimer, SIGNAL(timeout(void)), this, SLOT(grabFrame(void)));
}

FileSensor::~FileSensor () {
	delete []_buf;
	delete _pTimer;
}


void FileSensor::grabFrame () {
	if (!_isStarted)
		return;
	_pTimer->start (33);

	std::ostringstream sOut;
	clock_t start = clock ();
	_inStream.read (_buf, _frameDataSize);
	if (_inStream.eof ()) {
 		_inStream.seekg (0);
 		__sensorParameter.readFrom (_inStream);
		_inStream.read (_buf, _frameDataSize);
	}
	_hasNewFrame = true;
	bool result = false;
	if (_hasNewFrame) {
		if (!__depthMetaData.isNull ()) {
			memcpy ((void*)__depthMetaData.constBits (), _buf,
					__sensorParameter.frameSize *
							__sensorParameter.depthPixelWidth);
			for (unsigned int v = 0; v < __sensorParameter.frameHeight; ++v)
				for (unsigned int u = 0; u < __sensorParameter.frameWidth; ++u)
					if (__depthMetaData.pixel (QPoint (u, v)) == 0)
						__depthMetaData.setPixel (QPoint (u, v),
								__sensorParameter.frameDepth);
		}
		if (!__rgbMetaData.isNull ()) {
			memcpy ((void*)__rgbMetaData.constBits (),
					_buf + __sensorParameter.frameSize *
							__sensorParameter.depthPixelWidth,
					__sensorParameter.frameSize *
							__sensorParameter.rgbPixelWidth);
		}
		result = true;
	}
	clock_t end = clock ();
// 	sOut << getName () << ": New frame read: " << end - start << std::endl;
// 	OutputDebugStringA (sOut.str ().c_str ());
	if (result)
		__handleNewData ();
}

void FileSensor::start () {
	if (_isStarted)
		return;
	_isStarted = true;
	_pTimer->start (1);
}

void FileSensor::stop () {
	if (!_isStarted)
		return;
	_isStarted = false;
	_pTimer->stop ();
}